#include "stdafx.h"
#include "AccelerometerBase.h"
#include "Core/Commons.h"
//#include "Core/Float.h"

#include "Application.h"

//#include "EventManager/EventManager.h"
//#include "Game/Events/EvAcceleration.h"

#include "Game/Controls/AccelerometerControl.h"

#include <math.h>
#include <algorithm>


using namespace irr;
using namespace irr::scene;
using namespace irr::video;
using namespace irr::core;

// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Ctors / Dtor
// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AccelerometerBase::AccelerometerBase()
{
	m_x = m_y = m_z = 0.f;
	m_dx = m_dy = m_dz = 0.f;
	m_prevAngleXY = m_prevAngleYZ = m_prevAngleZX = 0.f;

	_calcAngles();
	clear();

	m_pEventReceiver = NULL;
}

AccelerometerBase::~AccelerometerBase()
{
}


// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Methods (Privates/Protected)
// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AccelerometerBase::_trace(const char* msg) const
{
	if(SHOW_TRACE)
	{
		//Logs::trace("AccelerometerBase", msg);
	}
}


void AccelerometerBase::_calcAngles()
{
	if(USE_ROLL)
	{
		m_prevAngleXY = m_angleXY;

		if(irr::core::iszero(m_x))	{m_angleXY = (m_prevAngleXY > 0.f) ? irr::core::HALF_PI : -irr::core::HALF_PI;}
		else							{m_angleXY = atan2(m_y, m_x);}
	}

	if(USE_PITCH)
	{
		m_prevAngleYZ = m_angleYZ;

		if(irr::core::iszero(m_y))	{m_angleYZ = (m_prevAngleYZ > 0.f) ? irr::core::HALF_PI : -irr::core::HALF_PI;}
		else							{m_angleYZ = atan2(m_z, m_y);}
	}

	if(USE_YAW)
	{
		m_prevAngleZX = m_angleZX;

		if(irr::core::iszero(m_z))	{m_angleZX = (m_prevAngleZX > 0.f) ? irr::core::HALF_PI : -irr::core::HALF_PI;}
		else							{m_angleZX = atan2(m_x, m_z);}
	}
}


// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Methods
// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AccelerometerBase::clear()
{
	m_minX = m_maxX = m_prevX = m_x;
	m_minX = m_maxX = m_prevY = m_y;
	m_minX = m_maxX = m_prevZ = m_z;
}


// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Methods (Events)
// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AccelerometerBase::moved(MOVE_TYPE x, MOVE_TYPE y, MOVE_TYPE z)
{
	//_trace("Moved!!");

	m_dx = m_x;
	m_dy = m_y;
	m_dz = m_z;

	m_x = x;
	m_y = y;
	m_z = z;

	m_dx = m_x - m_dx;
	m_dy = m_y - m_dy;
	m_dz = m_z - m_dz;

	//printf("x: %3.5f, y: %3.5f, z: %3.5f\n", m_x, m_y, m_z);

	m_minX = MAX(m_minX, x);	m_maxX = MAX(m_maxX, x);
	m_minY = MAX(m_minY, y);	m_maxY = MAX(m_maxY, y);
	m_minZ = MIN(m_minZ, z);	m_maxZ = MAX(m_maxZ, z);

	_calcAngles();

	if (m_pEventReceiver)
	{
		m_pEventReceiver->onEvent(m_x, m_y, m_z, m_angleXY, m_angleYZ, m_angleZX);
	}
}

//NS only one receiver at a time for accelerometer
void AccelerometerBase::RegisterForEvents(AccelerometerControl* cls)
{
	m_pEventReceiver = cls;
}

// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Methods (IUpdatable impl.)
// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AccelerometerBase::update(TimeStamp timestamp)
{
	clear();
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

int AccelerometerBase::GetPhoneRotSpeedFwdBack(irr::video::E_ORTHO_ORIENTATION orientation)
{
	float x = m_x;
	float y = m_y;
	float z = m_z;
	
	float dx = m_dx;
	float dy = m_dy;
	float dz = m_dz;
	
	float dAlpha = 0.0f;
	
	switch (orientation)
	{
		case irr::video::EOO_0:
			//angle = atan2f(-z, -y);

			if (z == 0 && y == 0) return 0;

			dAlpha = ((-dz) * (-y) - (-z) * (-dy)) / ( z * z + y * y);
			break;
			
		case irr::video::EOO_90:
			//angle = atan2f(-z, x);

			if (z == 0 && x == 0) return 0;

			dAlpha = ((-dz) * x - (-z) * dx) / ( z * z + x * x);
			break;
			
		case irr::video::EOO_180:
			//angle = atan2f(-z, y);

			if (z == 0 && y == 0) return 0;

			dAlpha = ((-dz) * y - (-z) * dy) / ( z * z + y * y);
			break;
			
		case irr::video::EOO_270:
			//angle = atan2f(-z, -x);

			if (z == 0 && x == 0) return 0;

			dAlpha = ((-dz) * (-x) - (-z) * (-dx)) / ( z * z + x * x);
			break;
	}
	
	return dAlpha * ACCEL_ONE; 	// rad per second
}